/**
  ****************************(C) COPYRIGHT 2019 DJI****************************
  * @file       obtein_task.c/h
  * @brief      夹取矿石任务
  * @note
  * @history
  *  Version    Date            Author          Modification
  *  V1.0.0     Dec-26-2021     RM              1. done
  *  V1.0.1     5/14/2022       yan fubing      optimize angle PID ALL
  @verbatim
  ==============================================================================
  ==============================================================================
  @endverbatim
  ****************************(C) COPYRIGHT 2019 DJI****************************
  */

#include "obtein_mineral_task.h"
#include "cmsis_os.h"
#include "bsp_buzzer.h"
#include "ins_task.h"
#include "bsp_rc.h"
#include "remote_control.h"
#include "detect_task.h"
#include "CAN_receive.h"
#include "obtein_mineral_behaviour.h"
#include "arm_math.h"
#include "user_lib.h"
int8_t give_friction_x_speed=0;
int8_t give_friction1_x_speed=0;
  /**
  * @brief          遥控器死区限制
  */
#define rc_deadband_limit(input, output, dealine)        \
    {                                                    \
        if ((input) > (dealine) || (input) < -(dealine)) \
        {                                                \
            (output) = (input);                          \
        }                                                \
        else                                             \
        {                                                \
            (output) = 0;                                \
        }                                                \
    }


  /**
	* @brief          夹取矿石任务初始化
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
static void engineering_obtein_init(engineering_obtein_move_t* engineer_obtein_move_init);
/**
  * @brief          获取矿石数据更新
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void engineering_obtein_feedback_update(engineering_obtein_move_t* engineer_obtein_move_data);
/**
  * @brief          设置状态
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void engineering_obtein_set_mode(engineering_obtein_move_t* engineer_set_mode);
/**
  * @brief          设置对应状态
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void engineering_obtein_set_mode_control(engineering_obtein_move_t* set_mode_control);
/**
  * @brief          给出控制电流
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void engineering_obtein_control_loop(engineering_obtein_move_t* engineer_control_loop);

/**
  * @brief          夹具校准计算，将校准记录的中值,最大 最小值
  * @param[out]     yaw 中值 指针
  * @param[out]     pitch 中值 指针
  * @param[out]     pitch 最大相对角度 指针
  * @param[out]     pitch 最小相对角度 指针
  * @retval         none
  */
//static void calc_clamp_cali(const engineer_step_cali_t* clamp_cali, uint16_t* clamp_offset, fp32* max_clamp, fp32* min_clamp);

extern engineer_work_e engineer_work;
//工程夹取运动
engineering_obtein_move_t engineering_obtein_move;
//工程夹取状态
engineer_obtein_state_e engineer_obtein_satae = engineering_Clamping_init;

  /**
	* @brief          夹取任务任务，间隔 CHASSIS_CONTROL_TIME_MS 2ms
	* @param[in]      pvParameters: 空
	* @retval         none
	*/
void obtein_mineral_task(void const* pvParameters)
{
	    //空闲一段时间
	    vTaskDelay(obtein_mineral_task_INIT_TIME);
	engineering_obtein_init(&engineering_obtein_move);
	while (toe_is_error(Clamp_turn_MOTOR1_TOE) || toe_is_error(Clamp_turn_MOTOR2_TOE) || toe_is_error(translation_motor_TOE) || toe_is_error(DBUS_TOE))
	{
		vTaskDelay(obtein_mineral_task_INIT_TIME);
	}
	while (1)
	{
		//更新电机以及其他传感器数据
		engineering_obtein_feedback_update(&engineering_obtein_move);
		//设置工程夹取部分控制模式
		engineering_obtein_set_mode(&engineering_obtein_move);
		//设置对应控制模式
		engineering_obtein_set_mode_control(&engineering_obtein_move);
		//循环控制
		engineering_obtein_control_loop(&engineering_obtein_move);

		if (!(toe_is_error(Clamp_turn_MOTOR1_TOE) && toe_is_error(Clamp_turn_MOTOR2_TOE) && toe_is_error(translation_motor_TOE)))
		{

			if (!engineering_obtein_move.can_chassis_A->button_limit_flg)
			{
				CAN_cmd_engineering(engineering_obtein_move.engineer_translation_motor_data.give_current, engineering_obtein_move.engineer_Clamp_turn_motor_data[0].give_current, engineering_obtein_move.engineer_Clamp_turn_motor_data[1].give_current);
				CAN_cmd_friction(engineering_obtein_move.engineer_frction_motor_data[0].give_current, engineering_obtein_move.engineer_frction_motor_data[1].give_current, \
					engineering_obtein_move.engineer_frction_motor_data[2].give_current, engineering_obtein_move.engineer_frction_motor_data[3].give_current);

				CAN_cmd_chassis_C(&engineering_obtein_move.transmit_chassis);   //发送底盘A板的控制数据
			}

			else
			{
				//发送控制电流
				CAN_cmd_engineering(engineering_obtein_move.engineer_translation_motor_data.give_current, engineering_obtein_move.engineer_Clamp_turn_motor_data[0].give_current, \
					engineering_obtein_move.engineer_Clamp_turn_motor_data[1].give_current);
				CAN_cmd_friction(engineering_obtein_move.engineer_frction_motor_data[0].give_current, engineering_obtein_move.engineer_frction_motor_data[1].give_current, \
					engineering_obtein_move.engineer_frction_motor_data[2].give_current, engineering_obtein_move.engineer_frction_motor_data[3].give_current);

				CAN_cmd_chassis_C(&engineering_obtein_move.transmit_chassis);  //发送底盘A板的控制数据
			}

		}
		//vTaskDelay(2);  //任务调度时间
	}
}

static void engineering_obtein_init(engineering_obtein_move_t* engineer_obtein_move_init)
{
	if (engineer_obtein_move_init == NULL)
	{
		return;
	}

	//横向臂pid
	const static fp32 M3508_turn_mile_pid[3] = { translation_ENCODE_RELATIVE_PID_KP,translation_ENCODE_RELATIVE_PID_KI,translation_ENCODE_RELATIVE_PID_KD };
	const static fp32 M3508_translation_speed_pid[3] = { M3508_translation_MOTOR_SPEED_PID_KP,M3508_translation_MOTOR_SPEED_PID_KI,M3508_translation_MOTOR_SPEED_PID_KD };
	//夹具旋转pid
	const static fp32 M3508_turn_angle_pid[3] = { clamp_ENCODE_RELATIVE_PID_KP, clamp_ENCODE_RELATIVE_PID_KI, clamp_ENCODE_RELATIVE_PID_KD };
	const static fp32 M3508_turn_speed_pid[3] = { M3508_Clamp_turn_MOTOR_SPEED_PID_KP,M3508_Clamp_turn_MOTOR_SPEED_PID_KI,M3508_Clamp_turn_MOTOR_SPEED_PID_KD };
	//摩擦轮pid
	const static fp32 M2006_fric_speed_pid[3] = { M2006_fric_PID_KP, M2006_fric_PID_KI, M2006_fric_PID_KD };
	//滤波
	const static fp32 engineer_translation_order_filter[1] = { engineering_translation_NUM };
	const static fp32 engineer_Clamp_turn_order_filter[1] = { engineering_Clamp_turn_NUM };
	const static fp32 engineer_friction_order_filter[1] = { engineering_friction_NUM };

	//工程开机夹取任务的初始状态
	engineer_obtein_move_init->engineer_obtein_state = engineering_Clamping_init;
	engineer_obtein_move_init->last_engineer_obtein_state = engineering_Clamping_init;

	engineer_obtein_move_init->Cylinder_Stretch_flg = 0;
	engineer_obtein_move_init->Cylinder_Clamp_flg = 0;

	engineer_obtein_move_init->engineering_obtein_RC = get_remote_control_point();

	//engineer_obtein_move_init->engineering_INS_angle = get_INS_angle_point();           //不需要A板陀螺仪

	engineer_obtein_move_init->transmit_chassis.rc_to_cahssis = get_remote_control_point();
	//获取夹取任务相关电机指针以及底盘升降限位
	engineer_obtein_move_init->can_chassis_A = get_chassis_A_point();
	engineer_obtein_move_init->engineer_translation_motor_data.motor_measure = get_translation_motor_measure_point();
	PID_init(&engineer_obtein_move_init->translation_motor_mileage_pid, PID_POSITION, M3508_turn_mile_pid, translation_ENCODE_RELATIVE_PID_MAX_OUT, translation_ENCODE_RELATIVE_PID_MAX_IOUT);

	PID_init(&engineer_obtein_move_init->engineer_translation_motor_pid, PID_POSITION, M3508_translation_speed_pid, M3508_translation_MOTOR_SPEED_MAX_OUT, M3508_translation_MOTOR_SPEED_MAX_IOUT);

	static uint8_t i;
	for (i = 0; i < 2; i++)
	{
		engineer_obtein_move_init->engineer_Clamp_turn_motor_data[i].motor_measure = get_Clamp_turn_motor_measure_point(i);

		PID_init(&engineer_obtein_move_init->clamp_relative_angle_pid[i], PID_POSITION, M3508_turn_angle_pid, clamp_ENCODE_RELATIVE_PID_MAX_OUT, clamp_ENCODE_RELATIVE_PID_MAX_IOUT);
		PID_init(&engineer_obtein_move_init->engineer_Clamp_turn_motor_pid[i], PID_POSITION, M3508_turn_speed_pid, M3508_Clamp_turn_MOTOR_SPEED_PID_MAX_OUT, M3508_Clamp_turn_MOTOR_SPEED_PID_MAX_IOUT);
	}
	for (i = 0; i < 4; i++)
	{
		engineer_obtein_move_init->engineer_frction_motor_data[i].motor_measure = get_friction_motor_measure_point(i);
		PID_init(&engineer_obtein_move_init->engineer_friction_motor_pid[i], PID_POSITION, M2006_fric_speed_pid, M2006_fric_PID_MAX_OUT, M2006_fric_PID_MAX_IOUT);
	}
	//用一阶滤波代替斜波函数生成
	first_order_filter_init(&engineer_obtein_move_init->engineer_cmd_slow_set_translation, engineering_CONTROL_TIME, engineer_translation_order_filter);
	first_order_filter_init(&engineer_obtein_move_init->engineer_cmd_slow_set_clamp_turn, engineering_CONTROL_TIME, engineer_Clamp_turn_order_filter);
	first_order_filter_init(&engineer_obtein_move_init->engineer_cmd_slow_set_friction_x, engineering_CONTROL_TIME, engineer_friction_order_filter);
	first_order_filter_init(&engineer_obtein_move_init->engineer_cmd_slow_set_friction_y, engineering_CONTROL_TIME, engineer_friction_order_filter);

	//ramp_init(&engineer_obtein_move_init->ramp,0.005,30,0);  //斜波初始化
		 //初始化夹爪电机角度以消除误差

	//最大速度
	engineer_obtein_move_init->translation_motor_max_speed = NORMAL_MAX_translation_SPEED;
	engineer_obtein_move_init->translation_motor_min_speed = -NORMAL_MAX_translation_SPEED;
	engineer_obtein_move_init->Clamp_turn_motor_max_speed = NORMAL_MAX_turn_SPEED;
	engineer_obtein_move_init->Clamp_turn_motor_min_speed = -NORMAL_MAX_turn_SPEED;
	engineer_obtein_move_init->friction_motor_max_speed = NORMAL_MAX_friction_SPEED;
	engineer_obtein_move_init->friction_motor_min_speed = -NORMAL_MAX_friction_SPEED;

	engineer_obtein_move_init->Clamp_Init_Angle = ((engineer_obtein_move_init->engineer_Clamp_turn_motor_data[1].motor_measure->ecd_count * ECD_RANGE) + engineer_obtein_move_init->engineer_Clamp_turn_motor_data[1].motor_measure->ecd) * MOTOR_ECD_TO_ANGLE3508;
	engineering_obtein_feedback_update(engineer_obtein_move_init);  //更新一下电机的初始化代码
	engineer_obtein_move_init->clamp_max_angle = engineer_obtein_move_init->Clamp_Init_Angle + 260;
	engineer_obtein_move_init->clamp_min_angle = engineer_obtein_move_init->Clamp_Init_Angle + 10;
	engineer_obtein_move_init->go_down_add_high_flag = 0;  //是否上升到兑换矿石的最高处（否）
	engineer_obtein_move_init->Chassis_Directions = 0;
}

/**
  * @brief          电机数据更新，主要在'chassis_behaviour_mode_set'函数中改变
  * @param[out]     chassis_move_mode:"chassis_move"变量指针.
  * @retval         none
  */
static void engineering_obtein_feedback_update(engineering_obtein_move_t* engineer_obtein_move_data)
{
	if (engineer_obtein_move_data == NULL)
	{
		return;
	}

	//int16_t clamp_turn_anger[2] = { 0 };
	/********************************更新夹具旋转电机参数*************************************************************/
	static uint8_t i;
	for (i = 0; i < 2; i++)
	{
		engineer_obtein_move_data->engineer_Clamp_turn_motor_data[i].speed = engineer_obtein_move_data->engineer_Clamp_turn_motor_data[i].motor_measure->speed_rpm / 18;
	}
	engineer_obtein_move_data->Clamp_turn_speed = (engineer_obtein_move_data->engineer_Clamp_turn_motor_data[0].speed - engineer_obtein_move_data->engineer_Clamp_turn_motor_data[1].speed) * MOTOR_SPEED_TO_enginner_clamp;
	//更新夹具当前角度位置,当前只计算一个电机的角度，右边夹具
	engineer_obtein_move_data->engineer_Clamp_turn_motor_data[1].angle
		= ((engineer_obtein_move_data->engineer_Clamp_turn_motor_data[1].motor_measure->ecd_count * ECD_RANGE) + engineer_obtein_move_data->engineer_Clamp_turn_motor_data[1].motor_measure->ecd) * MOTOR_ECD_TO_ANGLE3508;
	//横向机构电机角度初始化
	engineer_obtein_move_data->engineer_translation_motor_data.angle
		= ((engineer_obtein_move_data->engineer_translation_motor_data.motor_measure->ecd_count) * ECD_RANGE + engineer_obtein_move_data->engineer_translation_motor_data.motor_measure->ecd) * MOTOR_ECD_TO_ANGLE3508;

	engineer_obtein_move_data->clamp_turn_last_angle = (int16_t)(engineer_obtein_move_data->engineer_Clamp_turn_motor_data[1].angle);
	/******************************************获取横向臂电机参数***************************************************/
	engineer_obtein_move_data->engineer_translation_motor_data.speed = (engineer_obtein_move_data->engineer_translation_motor_data.motor_measure->speed_rpm / 18);
	engineer_obtein_move_data->engineer_translation_motor_data.accel = engineer_obtein_move_data->engineer_translation_motor_pid.Dbuf[0] * CHASSIS_CONTROL_FREQUENCE;
	//大致的取圈数来更新里程计,作为相对参考更加可靠
	engineer_obtein_move_data->translation_last_mileage = engineer_obtein_move_data->engineer_translation_motor_data.angle;
	/******************************************获取摩擦轮电机参数***************************************************/
	for (uint8_t f = 0; f < 4; f++)
	{
		engineer_obtein_move_data->engineer_frction_motor_data[f].speed = (engineer_obtein_move_data->engineer_frction_motor_data[f].motor_measure->speed_rpm) / 36;
		engineer_obtein_move_data->engineer_frction_motor_data[f].accel = engineer_obtein_move_data->engineer_friction_motor_pid->Dbuf[0] * CHASSIS_CONTROL_FREQUENCE;
	}
	if ((engineer_obtein_move_data->Cylinder_Stretch_flg) || (engineer_obtein_move_data->translation_last_mileage) || (engineer_obtein_move_data->clamp_turn_last_angle > 50))
	{
		engineer_obtein_move_data->last_engineer_obtein_state = engineering_Clamping_ready;
		//engineer_obtein_move_data->last_engineer_obtein_state = engineer_obtein_move_data->engineer_obtein_state;
	}
//	if(!read_Clamping_limit() && engineer_obtein_move_data->engineer_Clamp_turn_motor_data->angle < engineer_obtein_move_data->Clamp_Init_Angle + 80)
//	{
//		engineer_obtein_move_data->last_engineer_obtein_state = engineering_Clamping;
//	}
	engineer_obtein_move_data->transmit_chassis.state_flag |= 0xFF;
}

/**
  * @brief          设置状态
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void engineering_obtein_set_mode(engineering_obtein_move_t* engineer_set_mode)
{
	if (engineer_set_mode == NULL)
	{
		return;
	}
	engineer_behaviour_mode_set(engineer_set_mode);
}

/**
  * @brief          设置获取矿石状态
  * @param[in]      pvParameters: 空
  * @retval         none
  */
void engineering_obtein_set_state(engineering_obtein_move_t* engineer_set_state)
{
	if (engineer_set_state == NULL)
	{
		return;
	}

	if (engineer_set_state->engineering_obtein_RC->key.v & CLAMPING_CONTROL_KEY)           //对应矿石准备夹取
	{
		if (engineer_set_state->engineering_obtein_RC->key.v & CLAMPING_ONE_KEY )
		{
			
			engineer_set_state->Clamping_obtein = Clamping_1;
			engineer_set_state->engineer_obtein_state = engineering_Clamping_ready;
		}
		else if (engineer_set_state->engineering_obtein_RC->key.v & CLAMPING_TWO_KEY)
		{
			engineer_set_state->Clamping_obtein = Clamping_2;
			engineer_set_state->engineer_obtein_state = engineering_Clamping_ready;
		}
		else if (engineer_set_state->engineering_obtein_RC->key.v & CLAMPING_THREE_KEY)
		{
			engineer_set_state->Clamping_obtein = Clamping_3;
			engineer_set_state->engineer_obtein_state = engineering_Clamping_ready;
		}
	}
	
	//夹取任务回到初始状态
	if (engineer_set_state->engineering_obtein_RC->key.v & SAVE_OBTEIN_KEY)
	{
		engineer_set_state->engineer_obtein_state = engineering_Clamping_init;
	
	} 
	//开始夹取，气缸工作
	if (engineer_set_state->engineering_obtein_RC->mouse.press_l)
	{
		engineer_set_state->engineer_obtein_state = engineering_Clamping;
	}
	else if (engineer_set_state->engineering_obtein_RC->mouse.press_r)
	{
		engineer_set_state->engineer_obtein_state = engineering_No_Clamp;
	}
}

/**
  * @brief          设置控制，横向臂和夹具
  * @param[in]      pvParameters: 空
  * @retval         none
  */
void engineering_obtein_set_change(fp32* translation, fp32* clamp, engineering_obtein_move_t* engineer_set_change)
{
	if (engineer_set_change == NULL)
	{
		return;
	}    //夹取任务返回到初始状态


	if ((engineer_set_change->last_engineer_obtein_state != engineering_Clamping_init) && (engineer_set_change->engineer_obtein_state == engineering_Clamping_init))
	{

		*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle + 25;    //夹取中间角度
		*translation = 0;
		engineer_set_change->go_down_add_high_flag = 1;
		//			 if(engineer_set_change->can_chassis_A->button_limit_flg)
		//			 {
		if (abs(engineering_obtein_move.engineer_translation_motor_data.motor_measure->ecd_count) <= 4)
		{
			Cylinder_Stretch_reset();
			if (!read_Cylinder_Stretch_reset())   //霍尔传感器检测到收缩机构收回
			{			
				Cylinder_Clamp_reset();
        // *clamp = engineer_set_change->Clamp_Init_Angle+15;
//				if (!read_Clamping_limit())    //如果没有检测到夹取到了矿石
//				{
         
					*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle;      //如果霍尔传感器失效可以将此屏蔽，从而手动存放矿石(加几度是防止惯性)
					if (engineer_set_change->clamp_turn_last_angle >= engineer_set_change->Clamp_Init_Angle + clamping_middle_angle-10 )
					{
						engineer_set_change->Cylinder_Stretch_flg = 0;
						engineer_set_change->last_engineer_obtein_state = engineering_Clamping_init;
					}
					else return;
			//	}
				/*else return;*/
			}
			else return;
		}
		else return;
		// } else return;               
	}
	else if ((engineer_set_change->last_engineer_obtein_state != engineering_Clamping) && (engineer_set_change->engineer_obtein_state == engineering_Clamping))
	{
		static uint32_t time_pe=1000;
		//开始夹取或正在夹取，将完成夹具回归到45度，而后检查限位
		/*if (time_pe == 1000)
		{
			Cylinder_Clamp_set();
			time_pe = 0;
		}*/
		Cylinder_Clamp_set();
		engineer_set_change->Cylinder_Clamp_flg = 1;

		if (read_Clamping_limit())    //如果确实检测到夹取到了矿石
		{
			engineer_set_change->go_down_add_high_flag = 1;  //到最处的标志位为1
			/*if (engineer_set_change->can_chassis_A->top_limit_flg)
			{*/
				*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle ;
				if (engineer_set_change->clamp_turn_last_angle <= engineer_set_change->Clamp_Init_Angle + clamping_middle_angle + 25)
				{
					*translation = 0;
					engineer_set_change->go_down_add_high_flag = 2;  //到最处的标志位为零
					if (abs(engineering_obtein_move.engineer_translation_motor_data.motor_measure->ecd_count) <= 4)
					{
						Cylinder_Stretch_reset();
						if (!read_Cylinder_Stretch_reset())   //霍尔传感器检测到收缩机构收回
						{
							*clamp = engineer_set_change->Clamp_Init_Angle + 10.0f;  //如果霍尔传感器失效可以将此屏蔽，从而手动存放矿石(加几度是防止惯性)							
//							if (engineer_set_change->clamp_turn_last_angle <= engineer_set_change->Clamp_Init_Angle + 20)
//							{
//								Cylinder_Clamp_reset();
//								if (!read_Clamping_limit())    //如果确实检测到没有夹取到了矿石
//								{
//									//*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle;
								engineer_set_change->engineer_obtein_state = engineering_Clamping_init;
//									engineer_set_change->last_engineer_obtein_state = engineering_Clamping;
//									engineer_set_change->Cylinder_Stretch_flg = 1;
//								
//									engineer_set_change->Cylinder_Stretch_flg = 1;

//								}
//								return;
//							}
//							else return;
						}
						else return;
					}
					else return;
					/*}
					else return;*/
			}
			
		}
     else
			{

				engineer_set_change->go_down_add_high_flag = 0;  //到最处的标志位为0(未到)
				engineer_set_change->engineer_obtein_state = engineering_No_Clamp;
				engineer_set_change->last_engineer_obtein_state = engineering_Clamping;
			}  //由于本次没有夹取到，判定现在为夹取准备状态
	}
	else if ((engineer_set_change->last_engineer_obtein_state != engineering_No_Clamp) && (engineer_set_change->engineer_obtein_state == engineering_No_Clamp))
	{
		Cylinder_Clamp_reset();
		engineer_set_change->Cylinder_Clamp_flg = 0;
		engineer_set_change->last_engineer_obtein_state = engineering_No_Clamp;
	}
	else if (engineer_set_change->engineer_obtein_state == engineering_Clamping_ready)    //准备夹取
	{
		switch (engineer_set_change->Clamping_obtein)
		{
		case Clamping_1:
		{
			if ((engineer_set_change->last_Clamping_obtein != Clamping_1) || (engineer_set_change->last_engineer_obtein_state == engineering_Clamping_init))
			{
				*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle;    //夹取中间角度
				Cylinder_Stretch_set();
				engineer_set_change->Cylinder_Stretch_flg = 1;
				*translation = Clamping_1t2;
				if (engineer_set_change->engineer_translation_motor_data.motor_measure->ecd_count <= -110)
				{
					*clamp = clamping_set_angle;			//夹爪最终角度
					engineer_set_change->last_engineer_obtein_state = engineering_Clamping_ready;
					engineer_set_change->last_Clamping_obtein = Clamping_1;
				}
				else return;
			}
			else return;
			break;
		}
		case Clamping_2:
		{
			if ((engineer_set_change->last_Clamping_obtein != Clamping_2) || (engineer_set_change->last_engineer_obtein_state == engineering_Clamping_init) || 1)
			{
				*translation = 0;
				Cylinder_Stretch_set();
				*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle;    //夹取中间角度			 
				if (abs(engineering_obtein_move.engineer_translation_motor_data.motor_measure->ecd_count) <= 3)
				{
					*clamp = clamping_set_angle;
					engineer_set_change->last_Clamping_obtein = Clamping_2;
					engineer_set_change->Cylinder_Stretch_flg = 1;
					engineer_set_change->last_engineer_obtein_state = engineering_Clamping_ready;
				}
			}
			else return;
			break;
		}
		case Clamping_3:
		{
			if ((engineer_set_change->last_Clamping_obtein != Clamping_3) || (engineer_set_change->last_engineer_obtein_state == engineering_Clamping_init))
			{
				Cylinder_Stretch_set();
				*clamp = engineer_set_change->Clamp_Init_Angle + clamping_middle_angle;    //夹取中间角度			 
				*translation = Clamping_1t3;            //需要计算   

				if (engineer_set_change->engineer_translation_motor_data.motor_measure->ecd_count >= 110)
				{
					*clamp = clamping_set_angle;			//夹爪最终角度      
					engineer_set_change->last_engineer_obtein_state = engineering_Clamping_ready;
					engineer_set_change->last_Clamping_obtein = Clamping_3;
					engineer_set_change->Cylinder_Stretch_flg = 1;
				}
				else return;
			}
			else return;
			break;
		}
		default:
		{
			break;
		}
		}
	}
}

/**
  * @brief          兑换矿石
  * @param[in]      engineering_exchange_change: 兑换矿石状态
  * @retval         none
  */
void engineering_exchange_set_change(fp32* clamp, engineering_obtein_move_t* engineering_exchange_change)
{
	if (engineering_exchange_change->engineer_obtein_state == engineering_Clamping)
	{
		Cylinder_Stretch_reset();
		Cylinder_Clamp_set();
		if (read_Clamping_limit())
		{
			engineering_exchange_change->fric_exchange = 1;   //摩擦轮转动
			//*clamp = clamping_set_angle - 20.0f;  //需要计算
		}
		else return;
	}
	else if (engineering_exchange_change->engineer_obtein_state == engineering_No_Clamp)
	{
		//*clamp = 25;
		Cylinder_Clamp_reset();
		if (engineering_exchange_change->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_R)
		{
			Cylinder_Stretch_set();
		}
		else Cylinder_Stretch_reset();
	}
	else if (engineering_exchange_change->engineer_obtein_state == engineering_Clamping_init)
	{
		Cylinder_Stretch_reset();
		Cylinder_Clamp_reset();
		*clamp = engineering_exchange_change->Clamp_Init_Angle + 8.0f;
	}
}

/**
  * @brief          设置控制，单独控制部分
  * @param[in]      pvParameters: 空
  * @retval         none
  */
 
  void engineering_obtein_set_control(fp32* clamp, fp32* frc_x, fp32* frc_y, engineering_obtein_move_t* engineer_set_control)
  {
	  if (engineer_set_control == NULL || clamp == NULL || frc_x == NULL || frc_y == NULL)
	  {
		  return;
	  }
	 // int8_t go_down_channel = 0;
	  //夹具  调整
	  int8_t go_down_mileages = 0;
	 	if (!(engineer_set_control->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_SHIFT))
		{
			if (engineer_set_control->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_E)
			{
		  //engineer_set_control->engineer_obtein_state = engineering_Clamping_ready;			
		  if (engineer_set_control->clamp_turn_last_angle < engineer_set_control->clamp_max_angle)
		  {
			  *clamp += 0.004f;
		  }
		  }
	  
	    else if (engineer_set_control->engineering_obtein_RC->key.v & KEY_PRESSED_OFFSET_Q)
	    {
		  //	engineer_set_control->engineer_obtein_state = engineering_Clamping_ready;	
		  if (engineer_set_control->clamp_turn_last_angle >= engineer_set_control->clamp_min_angle)
		  {
			  *clamp -= 0.004f;
		  }
      }
	  }

	  //矿石  调整

	  if ((engineer_set_control->engineering_obtein_RC->key.v & FRICTIONY_KEY))
	  {
		  if (engineer_set_control->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)
		  {
			  *frc_y = engineer_set_control->friction_motor_max_speed;
			   give_friction_x_speed=60;
			    give_friction1_x_speed=-60;
		  }
	  }
	  else if ((engineer_set_control->engineering_obtein_RC->key.v & FRICTION_Y_KEY))
	  {
		  if (engineer_set_control->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)
		  {
			  *frc_y = -engineer_set_control->friction_motor_max_speed;
			   give_friction_x_speed=-60;
			    give_friction1_x_speed=60;
		  }
	  }
	  else if ((engineer_set_control->engineering_obtein_RC->key.v & FRICTIONX_KEY))
	  {
		  if (engineer_set_control->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)
		  {
			  *frc_x = engineer_set_control->friction_motor_max_speed;
              give_friction_x_speed=60;
		      	  give_friction1_x_speed=60;
			//   engineer_set_control->engineer_frction_motor_data[2].speed_set = 100;
			//   engineer_set_control->engineer_frction_motor_data[3].speed_set = -100;
		  }
		  
		  
	  }
	   else if ((engineer_set_control->engineering_obtein_RC->key.v & FRICTION_X_KEY))
	  {
		  if (engineer_set_control->engineering_obtein_RC->key.v & FRICTION_CONTROL_KEY)
		  {
			  *frc_x = -engineer_set_control->friction_motor_max_speed;
			  give_friction_x_speed=50;
			  give_friction1_x_speed=50;
			//   engineer_set_control->engineer_frction_motor_data[0].speed_set = 100;
			//   engineer_set_control->engineer_frction_motor_data[1].speed_set = -100;
		  }
		  
	  }
	  else
	  {
		  *frc_y = 0;
		  *frc_x = 0;
      give_friction_x_speed=0;
		  give_friction1_x_speed=0;
	  }


	  if (engineer_set_control->engineering_obtein_RC->key.v & SAVE_OBTEIN_KEY)
	  {
//		  if (read_friction_limit())  //检测到矿石到底
//		  {
//			engineer_set_control->fric_deposit = 0;
//			engineer_set_control->last_fric_deposit = 1;
//		  }
		 engineer_set_control->fric_deposit=1;
     engineer_set_control->fric_exchange = 0;
	  }
	   else if (engineer_set_control->engineering_obtein_RC->mouse.press_r)
	  {
       engineer_set_control->fric_exchange = 1;
       engineer_set_control->fric_deposit = 0;
	  }
	  else
	  {
      engineer_set_control->fric_deposit = 0;
		  engineer_set_control->fric_exchange = 0;
	  }

	  //升降控制
	  if (engineer_set_control->engineering_obtein_RC->key.v & GO_KEY)
	  {
		  engineer_set_control->go_down_add_high_flag = 0;
		  go_down_mileages = 35;
		  go_down_mileages = fp32_constrain(go_down_mileages, 0, GO_DOWN_MAX_HIGH);
		 
		  if (engineer_set_control->can_chassis_A->button_limit_flg)
		  {
			  engineer_set_control->transmit_chassis.go_down_speed = 0;
		  }

	  }
	  else if (engineer_set_control->engineering_obtein_RC->key.v & DOWN_KEY)
	  {
		  engineer_set_control->go_down_add_high_flag = 0;
		  go_down_mileages = -35;
		  go_down_mileages = fp32_constrain(go_down_mileages, -GO_DOWN_MAX_HIGH, 0);
	  }
	  else
	  {
		  go_down_mileages = 0;

	  }

		  //死区限制，因为遥控器可能存在差异 摇杆在中间，其值不为0     从这里取出RC的值    这里是遥控的通道
		  //rc_deadband_limit(engineer_set_control->engineering_obtein_RC->rc.ch[3], go_down_channel, CHASSIS_RC_DEADLINE);
	  engineer_set_control->transmit_chassis.go_down_speed = (int8_t)(engineer_set_control->engineering_obtein_RC->rc.ch[3] / RC_TO_CHASSIS_SPEED_SEN) + go_down_mileages;
  }

/**
  * @brief          设置对应状态
  * @param[in]      pvParameters: 空
  * @retval         none
  */
 
static void engineering_obtein_set_mode_control(engineering_obtein_move_t* set_mode_control)
{
	if (set_mode_control == NULL)
	{
		return;
	}

			static  fp32 translation_mileage = 0.0f;               //横向臂的里程
			static  fp32 clamp_angle         = 0.0f;               //夹具旋转的角度
	        static  fp32 friction_y_speed    = 0.0f;               //存矿用Y方向摩擦轮
		    static  fp32 friction_x_speed    = 0.0f;               //翻转矿石X轴方向 

	//获取几个电机的控制值
	engineer_behaviour_control_set(&translation_mileage, &clamp_angle, &friction_x_speed, &friction_y_speed, set_mode_control);
	//获取底盘控制量
	chassis_rc_control_set(set_mode_control);
	if (set_mode_control->engineer_motor_mode == engineer_motor_encode)
	{		
	    	//存矿判断取矿判断
			if(engineer_work == engineer_obtein)
		{
			
		/* 	if ((set_mode_control->fric_deposit) && !(set_mode_control->last_fric_deposit))
			{
				friction_y_speed = set_mode_control->friction_motor_max_speed;
				set_mode_control->last_fric_deposit = 1;
			}
			else if (read_friction_limit() && (set_mode_control->last_fric_deposit))
			{	
				friction_y_speed = 0;
				set_mode_control->last_fric_deposit = 1;
				set_mode_control->fric_deposit      = 0;
			} */

       /*********************************************************************************/

			if (set_mode_control->fric_deposit)
			{
				friction_y_speed = set_mode_control->friction_motor_max_speed;				
			}
			if (set_mode_control->fric_exchange)
			{
				friction_x_speed = set_mode_control->friction_motor_max_speed;
			}
		
		}
		else if (engineer_work == engineer_exchange)
		{
			
			if (set_mode_control->fric_deposit)
			{
				friction_y_speed = set_mode_control->friction_motor_max_speed;				
			}
				if (set_mode_control->fric_exchange)
			{
				friction_x_speed = set_mode_control->friction_motor_max_speed;
			}
		}

		/*      //滤波减速，暂时屏蔽
		first_order_filter_cali(&set_mode_control->engineer_cmd_slow_set_clamp_turn, clamp_speed);
		first_order_filter_cali(&set_mode_control->engineer_cmd_slow_set_translation, translation_speed);
		first_order_filter_cali(&set_mode_control->engineer_cmd_slow_set_friction_x, friction_x_speed);
		first_order_filter_cali(&setEEQQQQQ_mode_control->engineer_cmd_slow_set_friction_y, friction_y_speed);

		set_mode_control->Clamp_turn_speed_set = set_mode_control->engineer_cmd_slow_set_clamp_turn.out;
		set_mode_control->translation_speed_set = set_mode_control->engineer_cmd_slow_set_translation.out;
		set_mode_control->friction_x_speed_set = set_mode_control->engineer_cmd_slow_set_friction_x.out;
		set_mode_control->friction_y_speed_set = set_mode_control->engineer_cmd_slow_set_friction_y.out; */
		//目前只计算一个电机的角度环  输出在0-60

		set_mode_control->Clamp_turn_speed_set  = PID_calc(&set_mode_control->clamp_relative_angle_pid[1], set_mode_control->engineer_Clamp_turn_motor_data[1].angle,clamp_angle);
		//里程计pid		
		set_mode_control->translation_speed_set = PID_calc(&set_mode_control->translation_motor_mileage_pid,set_mode_control->engineer_translation_motor_data.angle, translation_mileage);
		set_mode_control->friction_x_speed_set = friction_x_speed;
		set_mode_control->friction_y_speed_set = friction_y_speed;
	}
	else
	{
		set_mode_control->engineer_Clamp_turn_motor_data[0].give_current = clamp_angle;        //夹取电机1
		set_mode_control->engineer_Clamp_turn_motor_data[1].give_current = -clamp_angle;       //夹取电机2
		set_mode_control->engineer_translation_motor_data.give_current = translation_mileage;  //横移电机
		set_mode_control->engineer_frction_motor_data[0].give_current = friction_y_speed;
		set_mode_control->engineer_frction_motor_data[1].give_current = friction_y_speed;
		set_mode_control->engineer_frction_motor_data[2].give_current = friction_x_speed;
		set_mode_control->engineer_frction_motor_data[3].give_current = friction_x_speed;
	}
}

/**
  * @brief          给出控制电流
  * @param[in]      pvParameters: 空
  * @retval         none
  */
static void engineering_obtein_control_loop(engineering_obtein_move_t* engineer_control_loop)
{
	if (engineer_control_loop == NULL || engineer_control_loop->engineer_motor_mode == engineer_motor_raw)
	{
		return;
	}
		engineer_control_loop->engineer_Clamp_turn_motor_data[0].speed_set = -engineer_control_loop->Clamp_turn_speed_set;
		engineer_control_loop->engineer_Clamp_turn_motor_data[1].speed_set =  engineer_control_loop->Clamp_turn_speed_set;	
		engineer_control_loop->engineer_translation_motor_data.speed_set   = engineer_control_loop->translation_speed_set;
	    //摩擦轮分解
	if ((engineer_work == engineer_obtein)|| (engineer_work == engineer_exchange))
	{
		
//		else
//		{
//			engineer_control_loop->engineer_frction_motor_data[0].speed_set = -give_friction_x_speed;
//			engineer_control_loop->engineer_frction_motor_data[1].speed_set = give_friction1_x_speed;
//			engineer_control_loop->engineer_frction_motor_data[2].speed_set = engineer_control_loop->friction_x_speed_set;
//			engineer_control_loop->engineer_frction_motor_data[3].speed_set = engineer_control_loop->friction_x_speed_set;
//		}



		if (engineer_control_loop->fric_exchange)
		{
			engineer_control_loop->engineer_frction_motor_data[0].speed_set = -(engineer_control_loop->friction_y_speed_set);
			engineer_control_loop->engineer_frction_motor_data[1].speed_set =  (engineer_control_loop->friction_y_speed_set);
			engineer_control_loop->engineer_frction_motor_data[2].speed_set = -(engineer_control_loop->friction_x_speed_set);
			engineer_control_loop->engineer_frction_motor_data[3].speed_set =  (engineer_control_loop->friction_x_speed_set);
		}
		else if (engineer_control_loop->fric_deposit)
		{
			engineer_control_loop->engineer_frction_motor_data[0].speed_set =  engineer_control_loop->friction_y_speed_set;
			engineer_control_loop->engineer_frction_motor_data[1].speed_set = -engineer_control_loop->friction_y_speed_set;
			engineer_control_loop->engineer_frction_motor_data[2].speed_set =  engineer_control_loop->friction_x_speed_set;
			engineer_control_loop->engineer_frction_motor_data[3].speed_set = -engineer_control_loop->friction_x_speed_set;
		}
    else
		{
			engineer_control_loop->engineer_frction_motor_data[0].speed_set = give_friction_x_speed;
			engineer_control_loop->engineer_frction_motor_data[1].speed_set = -give_friction1_x_speed;
			engineer_control_loop->engineer_frction_motor_data[2].speed_set = engineer_control_loop->friction_x_speed_set;
			engineer_control_loop->engineer_frction_motor_data[3].speed_set = engineer_control_loop->friction_x_speed_set;
		}
	}

	//计算PID 速度环
	static uint8_t i;
	for ( i=0; i<2; i++)
	{
	  PID_calc(&engineer_control_loop->engineer_Clamp_turn_motor_pid[i], engineer_control_loop->engineer_Clamp_turn_motor_data[i].motor_measure->speed_rpm, engineer_control_loop->engineer_Clamp_turn_motor_data[i].speed_set);
	}	    

	  //横向臂一个电机PID计算
	  PID_calc(&engineer_control_loop->engineer_translation_motor_pid, engineer_control_loop->engineer_translation_motor_data.motor_measure->speed_rpm, engineer_control_loop->engineer_translation_motor_data.speed_set);	
	
	for (i = 0; i <4; i++)
	{
		PID_calc(&engineer_control_loop->engineer_friction_motor_pid[i], engineer_control_loop->engineer_frction_motor_data[i].speed,engineer_control_loop->engineer_frction_motor_data[i].speed_set);
	}
	//赋值电流
	for (i=0; i<2; i++)
	{
		engineer_control_loop->engineer_Clamp_turn_motor_data[i].give_current = (int16_t)engineer_control_loop->engineer_Clamp_turn_motor_pid[i].out;
	}
    	engineer_control_loop->engineer_translation_motor_data.give_current   = (int16_t)engineer_control_loop->engineer_translation_motor_pid.out;
	for (i = 0; i <4; i++)
	{
		engineer_control_loop->engineer_frction_motor_data[i].give_current    = (int16_t)engineer_control_loop->engineer_friction_motor_pid[i].out;
	}
}

/**
  * @brief          夹具校准设置，将校准的 中值以及最小最大机械相对角度
  * @param[in]      yaw_offse:yaw 中值
  * @param[in]      pitch_offset:pitch 中值
  * @param[in]      max_yaw:pitch 最大相对角度
  * @param[in]      min_yaw:pitch 最小相对角度
  * @retval         返回空
  * @waring         这个函数使用到gimbal_control 静态变量导致函数不适用以上通用指针复用
  */
void set_cali_clamp_hook(const uint16_t clamp_ecd2, const fp32 max_clamp, const fp32 min_clamp)
{
     //待修正
	engineering_obtein_move.clamp_min_ced = clamp_ecd2;
	engineering_obtein_move.clamp_max_angle = max_clamp;
	engineering_obtein_move.clamp_min_angle = min_clamp;
}

/**
  * @brief          夹具校准计算，将校准记录的中值,最大 最小值返回
  * @param[out]     min_ced 最小ecd
  * @param[out]     max_clamp 最大相对角度 指针
  * @param[out]     min_clamp 最小相对角度 指针
  * @retval         返回1 代表成功校准完毕， 返回0 代表未校准完
  * @waring         这个函数使用到gimbal_control 静态变量导致函数不适用以上通用指针复用
  */
bool_t cmd_cali_clamp_hook(uint16_t* min_ced, fp32* max_clamp, fp32* min_clamp)
{
	if (engineering_obtein_move.engineer_cali.step == 0)
	{
		engineering_obtein_move.engineer_cali.step = engineer_CALI_START_STEP;
		engineering_obtein_move.engineer_cali.clamp_min_ced   = engineering_obtein_move.engineer_Clamp_turn_motor_data[1].motor_measure->ecd;
		engineering_obtein_move.engineer_cali.clamp_min_angle = engineering_obtein_move.clamp_turn_last_angle;
		engineering_obtein_move.engineer_cali.clamp_max_angle = engineering_obtein_move.clamp_turn_last_angle;
		return 0;
	}
	else if (engineering_obtein_move.engineer_cali.step == engineer_CALL_END_STEP)       //目前取得值是在数据更新里面的，后续待定
	{
		engineering_obtein_move.clamp_min_angle += 5.0f;
		engineering_obtein_move.clamp_max_angle -= 5.0f;
		engineering_obtein_move.engineer_cali.step = 0;
		return 1;
	}
	else
	{
		return 0;
	}
}

/****************************************the end**********************************************/

